#include "rosnode.h"
#include <QMessageBox>
#include "leiliconfig.h"
#include <visualization_msgs/Marker.h>
#include "util/topicfiled.h"


Q_DECLARE_METATYPE(nav_msgs::OccupancyGrid);

RosNode::RosNode(QObject *parent):QObject(parent)
{
     m_pubMap =  n.advertise<nav_msgs::OccupancyGrid>("map",100);
     m_pubMarker  = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
     m_pubPoint2 = n.advertise<sensor_msgs::PointCloud2>(TopicFiled::pcd2point2, 10);
}

RosNode::~RosNode()
{
    for(auto iter = m_mapData.begin(); iter != m_mapData.end(); iter++)
    {
        //todo delete;
    }
}




void RosNode::subMap()
{
    m_subMap = n.subscribe("map",100,&RosNode::callback,this);
}

void RosNode::pubMap(nav_msgs::OccupancyGrid &map)
{
    m_pubMap.publish(map);
}

nav_msgs::OccupancyGrid &RosNode::getMap()
{
    return m_map;
}


void RosNode::callback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    m_map.data = msg->data;
    m_map.info = msg->info;
    m_map.header = msg->header;
}

void RosNode::callback(const sensor_msgs::PointCloud2::ConstPtr &msg, QString topic)
{
    sensor_msgs::PointCloud2 *newData = new sensor_msgs::PointCloud2(*msg);

    if(auto iter = m_mapData.find(topic); iter != m_mapData.end())
    {
        sensor_msgs::PointCloud2 *p =  (sensor_msgs::PointCloud2 *)iter->second;
        delete p;

        iter->second = newData;
    }else{
        m_mapData[topic] = newData;
    }
}


void RosNode::subPose()
{
    for(int i =1; i <  LeiliCfg->getRobotNum() + 1; i++)
    {
         auto m_subPose = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>(QString("robot%1/checkPose").arg(i).toStdString(),100,boost::bind(&RosNode::subPoseCallback, this, _1, i));

         m_vecPose.push_back(m_subPose);

         auto pubPose =  n.advertise<geometry_msgs::PoseWithCovarianceStamped>(QString("robot%1/pose").arg(i).toStdString(), 100);
         m_vecPubPose.push_back(pubPose);
    }

}

void RosNode::subPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, int i)
{
    double x = msg->pose.pose.position.x;
    double y = msg->pose.pose.position.y;

    int grid_x,grid_y;

    RosNode::PoseSelect code =  RosNode::PoseSelect_Succ;

    do{
        bool succ = worldToMapGrid(x,y, grid_x,grid_y);
        if(!succ)
        {
            code = PoseSelect_NotInMap;
            break;
        }

        if(isObstacle(grid_x,grid_y))
        {
            code = PoseSelect_InObstacle;
            break;
        }
    }while(0);

    emit sigPoseSelectState(code,i);

    if(code != RosNode::PoseSelect_Succ)
    {
        auto iter = m_mapPose.find(i);
        if(iter != m_mapPose.end())
        {
            return;
        }

        m_mapPose.erase(i);
    }
    else{
        m_mapPose[i]= *msg;
    }
}

void RosNode::pubPose()
{
    for(auto iter = m_mapPose.begin(); iter != m_mapPose.end(); iter++)
    {
        m_vecPubPose[iter->first-1].publish(iter->second);
    }
}

void RosNode::getPos(std::vector<geometry_msgs::PoseWithCovarianceStamped> &vecPos)
{
    for(auto iter = m_mapPose.begin(); iter != m_mapPose.end(); iter++)
    {
        vecPos.push_back(iter->second);
    }
}

void RosNode::subPath()
{
    for(int i =1; i <  LeiliCfg->getRobotNum() + 1; i++)
    {
         std::string topic_string = "robot" + std::to_string(i) + "/path";
         auto p  = n.advertise<nav_msgs::Path>(topic_string, 10);
         m_vecPubPath.push_back(p);
    }
}

void RosNode::pubPath(std::vector<nav_msgs::Path> &path)
{
    if(path.size() < LeiliCfg->getRobotNum())
    {
        return;
    }

    for(int i = 0; i < LeiliCfg->getRobotNum(); ++i){
        m_vecPubPath[i].publish(path[i]);
    }
}

void RosNode::subPoint2(QString topic)
{
    //auto m_subPose = n.subscribe<sensor_msgs::PointCloud2>("aa",100,boost::bind(&RosNode::callback, this, _1, topic));
    const std::string strTopic = topic.toStdString();
    auto m_subPose = n.subscribe<sensor_msgs::PointCloud2>(strTopic,100,boost::bind(&RosNode::callback, this, _1, topic));


    m_mapSub.insert({topic,m_subPose});

}

sensor_msgs::PointCloud2 *RosNode::getPoint2Data(QString topic)
{
    if(auto iter  = m_mapData.find(topic); iter != m_mapData.end()){
        return (sensor_msgs::PointCloud2*)iter->second;
    }

    return nullptr;
}


void RosNode::pubMarker(std::vector<nav_msgs::Path> &path)
{
    for(auto i =0; i < path.size(); i++)
    {
        nav_msgs::Path& _path = path[i];
        QColor color =Qt::red;
        pubMarker(_path, i,visualization_msgs::Marker::POINTS,color);
    }
}

void RosNode::pubMarker(nav_msgs::Path &path, int markId, int32_t  type,QColor& color)
{
    visualization_msgs::Marker markPoints;

    markPoints.header.frame_id = path.header.frame_id;
    markPoints.id = markId;
    markPoints.action = visualization_msgs::Marker::ADD;
    markPoints.type = type;
    markPoints.scale.x = 0.2;
    markPoints.scale.y = 0.2;

    markPoints.color.r = color.redF();
    markPoints.color.g = color.greenF();
    markPoints.color.b = color.blueF();

    markPoints.color.a = 1.0;
    markPoints.lifetime = ros::Duration(-1);

    for(int j =0;  j < path.poses.size(); j++)
    {
        auto& pose = path.poses[j];


        geometry_msgs::Point p;
        p.x = pose.pose.position.x;
        p.y = pose.pose.position.y;
        p.z = pose.pose.position.z;

        markPoints.points.push_back(p);
    }

    m_pubMarker.publish(markPoints);
}

template<typename T>
void RosNode::callBack(const typename T::ConstPtr &msg, QString topic)
{
    T *newData = new T(*msg);

    if(auto iter = m_mapData.find(topic); iter != m_mapData.end())
    {
        T *p =  (T *)iter->second;
        delete p;

        iter->second = newData;
    }else{
        m_mapData[topic] = newData;
    }

    emit sigTopicRecive(topic);
}

template <typename T>
void RosNode::sub(QString topic)
{
    auto sub = n.advertise<T>(topic.toStdString(), 10,boost::bind(&RosNode::callBack<T>, this, _1, topic));
    m_mapSub.insert({topic,sub});
}

template<typename T>
T &RosNode::get(QString topic)
{
    if(auto iter  = m_mapData.find(topic); iter != m_mapData.end()){
        return (T*)iter->second;
    }

    return nullptr;
}

void RosNode::pubPoint2(sensor_msgs::PointCloud2 &p)
{
    m_pubPoint2.publish(p);
}


bool RosNode::isObstacle(unsigned int x, unsigned int y)
{
    // 检查坐标是否在地图范围内
    if (x >= m_map.info.width || y >= m_map.info.height) {
        return false;
    }

    // 计算一维数组索引
    unsigned int index = y * m_map.info.width + x;

    // 获取栅格值
    int8_t value = m_map.data[index];
    //QMessageBox::information(nullptr, "超出地图范围",QString("value=%1").arg(value));

    // 判断是否为障碍物(通常≥50视为障碍物)
    return value  !=0;
}

bool RosNode::worldToMapGrid(double world_x, double world_y, int &grid_x, int &grid_y)
{
    // 1. 减去地图原点偏移
    double map_x = world_x - m_map.info.origin.position.x;
    double map_y = world_y - m_map.info.origin.position.y;

    // 2. 转换为网格坐标（四舍五入到最近的整数）
    grid_x = static_cast<int>(map_x / m_map.info.resolution + 0.5);
    grid_y = static_cast<int>(map_y / m_map.info.resolution + 0.5);

    // 3. 检查是否在地图范围内
    if (grid_x < 0 || grid_y < 0 ||
        static_cast<unsigned int>(grid_x) >= m_map.info.width ||
        static_cast<unsigned int>(grid_y) >= m_map.info.height)
    {
        //QMessageBox::information(nullptr, "超出地图范围",QString("grid_x=%1,grid_y=%2").arg(grid_x).arg(grid_y));
        return false; // 超出地图范围
    }

    //QMessageBox::information(nullptr, "超出地图范围",QString("grid_x=%1,grid_y=%2").arg(grid_x).arg(grid_y));
    return true;
}




